//
// Created by Shin on 2023/11/7.
//

#include "BTW901.h"
#include "../Sensor.h"
#include "usart.h"

#if USE_WIT
#define WIT_IO  huart2

void WIT_reset(void)
{
    print_UARTx(&WIT_IO, "%c%c%c%c%c", 0xFF, 0xAA, 0x69, 0x88, 0xB5);
    HAL_Delay(1000);
    print_UARTx(&WIT_IO, "%c%c%c%c%c", 0xFF, 0xAA, 0x69, 0x88, 0xB5);
    HAL_Delay(3000);
    print_UARTx(&WIT_IO, "%c%c%c%c%c", 0xFF, 0xAA, 0x69, 0x88, 0xB5);
}

short WIT_transform_data(uint8_t DATA_H, uint8_t DATA_L)
{
    return (short)((short)DATA_H<<8|DATA_L);
}

uint8_t WIT_data_sum_crc(uint8_t *data, int data_num)
{
    uint8_t sum;
    for(int j = 0; j < data_num; j++)
    {
        sum = 0;
        if(data[11 * j] != 0x55)
            return 0;
        for (int i = 0; i < 10; i++) {
            sum += data[11 * j + i];
        }
//        print_UARTx(&huart1, "%d\r\n", data[11 * j + 1]);
        if(data[11 * j + 10] != sum)
            return 0;
    }
    return 1;
}

void WIT_data_update(uint8_t *data_source)
{
    short version;
    switch (data_source[1]) {
        case WIT_KIND_LINEAR_ACCELERATION:
//            print_UARTx(&huart1, "hit line acc\r\n");
            ros_imu.accel[0] = (float)(16 * 9.8 * WIT_transform_data(data_source[3], data_source[2]) / 32768);
            ros_imu.accel[1] = (float)(16 * 9.8 * WIT_transform_data(data_source[5], data_source[4]) / 32768);
            ros_imu.accel[2] = (float)(16 * 9.8 * WIT_transform_data(data_source[7], data_source[6]) / 32768);
            ros_imu.is_latest[2] = 1;
            break;
        case WIT_KIND_ANGULAR_VELOCITY:
//            print_UARTx(&huart1, "hit angle vel\r\n");
            ros_imu.gyro[0] = (2000.0f * (float)WIT_transform_data(data_source[3], data_source[2]) / 32768);
            ros_imu.gyro[1] = (2000.0f * (float)WIT_transform_data(data_source[5], data_source[4]) / 32768);
            ros_imu.gyro[2] = (2000.0f * (float)WIT_transform_data(data_source[7], data_source[6]) / 32768);
            ros_imu.is_latest[1] = 1;
            break;
        case WIT_KIND_ANGLE:
//            print_UARTx(&huart1, "hit angle\r\n");
            ros_imu.roll    = 180.0f * (float)WIT_transform_data(data_source[3], data_source[2]) / 32768;
            ros_imu.pitch   = 180.0f * (float)WIT_transform_data(data_source[5], data_source[4]) / 32768;
            ros_imu.yaw     = 180.0f * (float)WIT_transform_data(data_source[7], data_source[6]) / 32768;
            ros_imu.is_latest[0] = 1;
            break;
        case WIT_KIND_ORIENTATION:
//            print_UARTx(&huart1, "hit ori\r\n");
            ros_imu.quat[0] = (float)WIT_transform_data(data_source[3], data_source[2]) / 32768;
            ros_imu.quat[1] = (float)WIT_transform_data(data_source[5], data_source[4]) / 32768;
            ros_imu.quat[2] = (float)WIT_transform_data(data_source[7], data_source[6]) / 32768;
            ros_imu.quat[3] = (float)WIT_transform_data(data_source[9], data_source[8]) / 32768;
            ros_imu.is_latest[3] = 1;
            break;
        case WIT_KIND_VERSION:
//            print_UARTx(&huart1, "hit version\r\n");
            version = WIT_transform_data(data_source[3], data_source[2]);
            if(version == WIT_VERSION)
                ros_imu.is_alive = 1;
            else
                ros_imu.is_alive = 0;

    }
}

#endif
